/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.Encoder;

/**
 *
 * @author Administrator
 */
class Autonomous {

    private Encoder leftEncoder = new Encoder(3, 4);
    private Encoder rightEncoder = new Encoder(5, 6);
    private Howie howie;
    private int currentStep;
    private boolean doneStep;

    public void Autonomous(Howie robot) {
        howie = robot;
        leftEncoder.reset();
        rightEncoder.reset();
    }

    public void runRoutine(String Routine) {
        if (Routine.equals("forward")) {
            switch (currentStep) {
                case 1:
                    moveForward(1000);
                    if (doneStep) {
                        advanceStep();
                    }
                    break;
                case 2:
                    return;
            }
        }
        if (Routine.equals("bacilli")) {
        }
    }

    public void moveForward(double targetCount) {
        if (leftEncoder.getRaw() > targetCount) {
            doneStep = true;
        }
    }

    private void advanceStep() {
        reset();
        currentStep++;
    }

    private void reset() {
        leftEncoder.reset();
        rightEncoder.reset();
    }
}
